/*
 * Route.cpp
 *
 *  Created on: Mar 13, 2011
 *      Author: kmueller
 */

#include "Route.h"

#include "Geo.h"
#include "Road.h"

#include <vector>

using namespace std;

namespace opennav {

Route::Route() {
	length_ = 0;
}

void Route::add_segment(RouteSegment& segment) {
	length_ += segment.length();
	segments_.insert(segments_.begin(), segment);
}

const Coordinate& Route::destination() const {
	return segments_.back().end_junction().coordinate;
}

const Coordinate& Route::start() const {
	return segments_.front().start_junction().coordinate;
}

int32_t Route::length() const {
	return length(NULL);
}

int32_t Route::length(const RoadSegment* segment) const {
	if(segment == NULL){
		return length_;
	}
	int32_t length = 0;

	vector<RouteSegment> tmp = segments_; // TODO: don't know wht we have to do this work-around. fix it in the next version

	for (vector<RouteSegment>::iterator it = tmp.begin(); it != tmp.end(); it++) {
		if (*segment == it->road_segment()){
			length = 0;
		}
		length += it->length();
	}
	return length;
}



int32_t Route::travel_time(const TravelTimeUnit unit = MINUTES) const {
	return travel_time(NULL, unit);
}

int32_t Route::travel_time(const RoadSegment* segment, const TravelTimeUnit unit = MINUTES) const {
	int32_t traveltime = 0;

	vector<RouteSegment> tmp = segments_; // TODO: don't know wht we have to do this work-around. fix it in the next version

	for (vector<RouteSegment>::iterator it = tmp.begin(); it != tmp.end(); it++) {
		if (segment != NULL &&  *segment == it->road_segment()){
			traveltime = 0;
		}
		traveltime += it->travel_time(SECONDS);
	}

	switch (unit) {
	case HOURS:
		traveltime /= 60;
	case MINUTES:
		traveltime /= 60;
	case SECONDS:
		return traveltime;
	}
	return traveltime;
}

const vector<RouteSegment>& Route::segments() const {
	return segments_;
}

RouteSegment::RouteSegment(const RoadJunction& from, const RoadSegment& via,
		const RoadJunction& to) {
	src = from;
	segment = via;
	dst = to;
}

const RouteSegment::RouteInstruction RouteSegment::instruction() const {
	// TODO:
	return CONTINUE_STRAIGHT;
}

int32_t RouteSegment::length() const {
	return segment.length;
}

int32_t RouteSegment::travel_time(const TravelTimeUnit unit = SECONDS) const {
	int maxspeed = segment.get_max_speed();

	float seconds = (segment.length) / (maxspeed / 3.6);
	switch (unit) {
	case HOURS:
		seconds /= 60;
	case MINUTES:
		seconds /= 60;
	case SECONDS:
		return seconds;
	}
	return seconds;
}

const RoadJunction& RouteSegment::start_junction() const {
	return src;
}

const RoadJunction& RouteSegment::end_junction() const {
	return dst;
}

const RoadSegment& RouteSegment::road_segment() const {
	return segment;
}

}
